#include "LD2451.h"
#include <cstring>

// 初始化静态成员
std::vector<uint8_t> LD2451::dummyResponse_;

// 帧头帧尾定义
const uint8_t LD2451::HEADER[4] = {0xF4, 0xF3, 0xF2, 0xF1};
const uint8_t LD2451::FOOTER[4] = {0xF8, 0xF7, 0xF6, 0xF5};
const uint8_t LD2451::CMD_HEADER[4] = {0xFD, 0xFC, 0xFB, 0xFA};
const uint8_t LD2451::CMD_FOOTER[4] = {0x04, 0x03, 0x02, 0x01};

LD2451::LD2451(HardwareSerial& serial) : 
    serial_(&serial), 
    frameState(WAITING_HEADER), 
    currentTargetCount(0),
    lastFrameTime(0),
    configEnabled(false),
    expectedLength(0),
    bytesRead(0)
{}

void LD2451::begin(uint32_t baudrate) {
    serial_->begin(baudrate);
}

void LD2451::processSerial() {
    while (serial_->available()) {
        uint8_t byte = serial_->read();
        processData(byte);
    }
}

const std::vector<LD2451::Target>& LD2451::getTargets() const {
    return targets;
}

unsigned long LD2451::getLastFrameTime() const {
    return lastFrameTime;
}

uint8_t LD2451::getTargetCount() const {
    return currentTargetCount;
}

// ================ 配置命令接口实现 ================

bool LD2451::enableConfigMode() {
    if (sendCommand(0x00FF, {0x01, 0x00})) {
        configEnabled = true;
        return true;
    }
    return false;
}

bool LD2451::endConfigMode() {
    if (sendCommand(0x00FE)) {
        configEnabled = false;
        return true;
    }
    return false;
}

bool LD2451::setDetectionParams(const Config& config) {
    if (!configEnabled) return false;
    
    std::vector<uint8_t> data = {
        config.minDistance,
        config.detectionDirection,
        config.minSpeed,
        config.noTargetDelay
    };
    return sendCommand(0x0002, data);
}

bool LD2451::readDetectionParams(Config& config) {
    if (!configEnabled) return false;
    
    std::vector<uint8_t> response;
    if (sendCommand(0x0012, {}, response) && response.size() >= 4) {
        config.minDistance = response[0];
        config.detectionDirection = response[1];
        config.minSpeed = response[2];
        config.noTargetDelay = response[3];
        return true;
    }
    return false;
}

bool LD2451::setSensitivityParams(const Sensitivity& sens) {
    if (!configEnabled) return false;
    
    std::vector<uint8_t> data = {
        sens.triggerCount,
        sens.snrThreshold,
        0x00,  // 扩展参数
        0x00   // 扩展参数
    };
    return sendCommand(0x0003, data);
}

bool LD2451::readSensitivityParams(Sensitivity& sens) {
    if (!configEnabled) return false;
    
    std::vector<uint8_t> response;
    if (sendCommand(0x0013, {}, response) && response.size() >= 2) {
        sens.triggerCount = response[0];
        sens.snrThreshold = response[1];
        return true;
    }
    return false;
}

bool LD2451::readFirmwareVersion(String& version) {
    if (!configEnabled) return false;
    
    std::vector<uint8_t> response;
    if (sendCommand(0x00A0, {}, response) && response.size() >= 8) {
        // 解析固件版本: 类型(2) + 主版本(2) + 次版本(4)
        uint16_t type = response[0] | (response[1] << 8);
        uint16_t major = response[2] | (response[3] << 8);
        uint32_t minor = 0;
        for (int i = 0; i < 4; i++) {
            minor |= (static_cast<uint32_t>(response[4 + i]) << (i * 8));
        }
        
        char buf[20];
        snprintf(buf, sizeof(buf), "V%d.%02d.%08X", major, minor >> 16, minor & 0xFFFF);
        version = String(buf);
        return true;
    }
    return false;
}

bool LD2451::setBaudRate(uint16_t baudIndex) {
    if (!configEnabled) return false;
    
    std::vector<uint8_t> data = {
        static_cast<uint8_t>(baudIndex & 0xFF),
        static_cast<uint8_t>((baudIndex >> 8) & 0xFF)
    };
    return sendCommand(0x00A1, data);
}

bool LD2451::factoryReset() {
    if (!configEnabled) return false;
    return sendCommand(0x00A2);
}

bool LD2451::reboot() {
    if (!configEnabled) return false;
    return sendCommand(0x00A3);
}

// ================ 私有方法实现 ================

void LD2451::processData(uint8_t byte) {
    switch (frameState) {
        case WAITING_HEADER:
            handleHeader(byte);
            break;
        case READING_LENGTH:
            readFrameLength(byte);
            break;
        case READING_PAYLOAD:
            readPayload(byte);
            break;
        case CHECKING_FOOTER:
            checkFooter(byte);
            break;
    }
}

void LD2451::handleHeader(uint8_t byte) {
    static uint8_t headerIndex = 0;
    
    if (byte == HEADER[headerIndex]) {
        headerIndex++;
        buffer.push_back(byte);
        
        if (headerIndex == 4) {
            frameState = READING_LENGTH;
            headerIndex = 0;
        }
    } else {
        headerIndex = 0;
        buffer.clear();
    }
}

void LD2451::readFrameLength(uint8_t byte) {
    buffer.push_back(byte);
    
    if (buffer.size() == 6) { // 帧头(4) + 长度(2)
        // 小端格式读取长度
        expectedLength = buffer[4] | (buffer[5] << 8);
        bytesRead = 0;
        
        // 最小长度检查（目标数量+报警信息）
        if (expectedLength >= 2) {
            frameState = READING_PAYLOAD;
        } else {
            reset(); // 无效长度
        }
    }
}

void LD2451::readPayload(uint8_t byte) {
    buffer.push_back(byte);
    bytesRead++;
    
    if (bytesRead >= expectedLength) {
        frameState = CHECKING_FOOTER;
    }
}

void LD2451::checkFooter(uint8_t byte) {
    static uint8_t footerIndex = 0;
    static uint8_t footerBuffer[4] = {0};
    
    footerBuffer[footerIndex] = byte;
    footerIndex++;
    
    if (footerIndex == 4) {
        // 验证帧尾
        if (memcmp(footerBuffer, FOOTER, 4) == 0) {
            // 完整帧接收成功
            parseTargets();
            lastFrameTime = millis();
        }
        
        reset(); // 无论成功与否都重置状态
        footerIndex = 0;
    }
}

void LD2451::parseTargets() {
    targets.clear();
    
    // 最小帧检查：帧头(4) + 长度(2) + 目标数量(1) + 报警信息(1) + 帧尾(4)
    if (buffer.size() < 12) return;
    
    // 获取目标数量和报警信息
    currentTargetCount = buffer[6];
    // uint8_t alarmInfo = buffer[7]; // 可选：处理报警信息
    
    // 解析每个目标（每个目标5字节）
    size_t pos = 8; // 数据起始位置
    for (int i = 0; i < currentTargetCount; i++) {
        if (pos + 5 > buffer.size() - 4) break; // 防止越界
        
        Target target;
        
        // 角度计算（带符号）
        target.angle = static_cast<int8_t>(buffer[pos++] - 0x80);
        
        // 距离（米）
        target.distance = buffer[pos++];
        
        // 运动方向（1字节）和速度（1字节）
        uint8_t dirByte = buffer[pos++];
        target.speed = buffer[pos++]; // 速度值
        
        // 方向解析：00=远离，01=靠近 (协议规定)
        target.isApproaching = (dirByte == 0x01);
        
        // 信噪比
        target.snr = buffer[pos++];
        
        targets.push_back(target);
    }
}

void LD2451::reset() {
    frameState = WAITING_HEADER;
    buffer.clear();
    expectedLength = 0;
    bytesRead = 0;
}

bool LD2451::sendCommand(uint16_t cmd, const std::vector<uint8_t>& data, 
                        std::vector<uint8_t>& response, 
                        unsigned long timeout) {
    // 构建命令帧
    std::vector<uint8_t> frame;
    
    // 帧头
    for (int i = 0; i < 4; i++) {
        frame.push_back(CMD_HEADER[i]);
    }
    
    // 数据长度 (命令字2字节 + 数据长度)
    uint16_t dataLength = 2 + data.size();
    frame.push_back(static_cast<uint8_t>(dataLength & 0xFF));
    frame.push_back(static_cast<uint8_t>((dataLength >> 8) & 0xFF));
    
    // 命令字 (小端)
    frame.push_back(static_cast<uint8_t>(cmd & 0xFF));
    frame.push_back(static_cast<uint8_t>((cmd >> 8) & 0xFF));
    
    // 命令数据
    for (uint8_t b : data) {
        frame.push_back(b);
    }
    
    // 帧尾
    for (int i = 0; i < 4; i++) {
        frame.push_back(CMD_FOOTER[i]);
    }
    
    // 发送命令
    serial_->write(frame.data(), frame.size());
    serial_->flush();
    
    // 等待ACK响应
    return waitForACK(cmd, response, timeout);
}

bool LD2451::waitForACK(uint16_t cmd, std::vector<uint8_t>& response, unsigned long timeout) {
    unsigned long start = millis();
    std::vector<uint8_t> ackBuffer;
    uint8_t state = 0; // 0:等待帧头, 1:读取长度, 2:读取数据, 3:检查帧尾
    uint16_t ackLength = 0;
    uint16_t bytesRead = 0;
    
    response.clear(); // 清空响应
    
    while (millis() - start < timeout) {
        if (serial_->available()) {
            uint8_t byte = serial_->read();
            
            switch (state) {
                case 0: // 等待帧头
                    if (ackBuffer.size() < 4) {
                        if (byte == CMD_HEADER[ackBuffer.size()]) {
                            ackBuffer.push_back(byte);
                        } else {
                            ackBuffer.clear();
                        }
                    }
                    if (ackBuffer.size() == 4) {
                        state = 1;
                    }
                    break;
                    
                case 1: // 读取长度
                    ackBuffer.push_back(byte);
                    if (ackBuffer.size() == 6) {
                        ackLength = ackBuffer[4] | (ackBuffer[5] << 8);
                        bytesRead = 0;
                        state = 2;
                    }
                    break;
                    
                case 2: // 读取数据
                    ackBuffer.push_back(byte);
                    bytesRead++;
                    if (bytesRead >= ackLength) {
                        state = 3;
                    }
                    break;
                    
                case 3: // 检查帧尾
                    ackBuffer.push_back(byte);
                    if (ackBuffer.size() >= 10) { // 最小ACK帧长度
                        // 检查帧尾：最后4字节
                        size_t endPos = ackBuffer.size() - 4;
                        if (endPos + 4 <= ackBuffer.size()) {
                            bool footerMatch = true;
                            for (int i = 0; i < 4; i++) {
                                if (ackBuffer[endPos + i] != CMD_FOOTER[i]) {
                                    footerMatch = false;
                                    break;
                                }
                            }
                            if (footerMatch) {
                                // 验证命令字
                                if (ackBuffer.size() >= 8) {
                                    uint16_t ackCmd = ackBuffer[6] | (ackBuffer[7] << 8);
                                    // 协议规定：ACK命令字 = 发送命令字 & 0x0100
                                    if ((ackCmd & 0xFF) == (cmd & 0xFF) && (ackCmd >> 8) == 0x01) {
                                        // 提取响应数据（从第8字节开始到帧尾前4字节）
                                        if (ackLength > 2) { // 有数据
                                            response = std::vector<uint8_t>(ackBuffer.begin() + 8, ackBuffer.begin() + 8 + (ackLength - 2));
                                        }
                                        // 检查ACK状态（如果有状态字段）
                                        if (ackLength >= 2) {
                                            // 状态在数据部分的前2字节
                                            uint16_t status = ackBuffer[8] | (ackBuffer[9] << 8);
                                            return (status == 0); // 0表示成功
                                        }
                                        return true; // 没有状态字段，直接返回成功
                                    }
                                }
                            }
                        }
                        return false; // 无效ACK
                    }
                    break;
            }
        }
    }
    return false; // 超时
}